/*! \file	extractor.hpp
 *  \brief	Declarations for the EXTRACTOR node.
 * 			Thanks to Hans Moorkens for contributions to this node.
*/

#ifndef _THERMALVIS_EXTRACTOR_H_
#define _THERMALVIS_EXTRACTOR_H_
	
#include "ros_resources.hpp"
#include "general_resources.hpp"
#include "opencv_resources.hpp"
#include "tools.hpp"
#include "improc.hpp"

#include <dynamic_reconfigure/server.h>
#include "extractorConfig.h"

#include <rosbag/bag.h>
#include <rosbag/view.h>

#include <message_filters/subscriber.h>
#include <message_filters/synchronizer.h>
#include <message_filters/sync_policies/approximate_time.h>

#include <opencv/cv.h>
#include <opencv/highgui.h>

#include <cv_bridge/cv_bridge.h> 
#include <image_transport/image_transport.h>

#include <boost/foreach.hpp>
#include "boost/filesystem.hpp"  
#include <time.h>
#include <math.h>

#include "ros/ros.h"
#include "sensor_msgs/Image.h"
#include "sensor_msgs/CameraInfo.h"

#include <iostream>
#include <fstream>

#define SAVEPLY false
#define FILTER_DISTANCE 1
#define SIGMA_P 15
#define SIGMA_R .05

#define MAX_BAGS 200

#define DEFAULT_MAX_TIMESTAMP_DIFF	0.01

const char __PROGRAM__[] = "EXTRACTOR";
using namespace cv;
/// \brief		Stores configuration information for the rosbag extractor routine
struct extractorData {
	
	string bagSource, outputFolder;
	
	bool debugMode, verboseMode;
	
	bool internalTimestamp, thermistorReading, recordDuplicates, undistortThermal;
	
	bool setupForMapping;
	
	bool splitBags;
	
	string thermalIntrinsics;
	
	int maxFrames;
	
	string rgbTopic;
	string depthTopic;
	string thermalTopic;
	
	bool rawTimeFormat;
	
	// Intra-determined variables
	
	bool wantsThermal, wantsRgb, wantsDepth;
	
	string rgbInfoTopic;
	string depthInfoTopic;
	string thermalInfoTopic;
	
	string read_addr;
	
	bool obtainStartingData(ros::NodeHandle& nh);  
	
};

/// \brief		Manages the extractor procedure
class extractorNode {
private:

	ros::NodeHandle *ref;
	
	// cam-calib
	Mat imageSize, cameraMatrix, distCoeffs, newCamMat;
	Size cameraSize;
	Mat map1, map2;

	
	Mat oldThermal;
	
	rosbag::Bag bag[MAX_BAGS];
	
	vector<string> splitBagNames;
	
	float *writeData;
	
	extractorData configData;
	
	bool stillListening;
	
	ofstream ofs_thermistor_log;
	string thermistorLogFile;
	
	ros::NodeHandle private_node_handle;
	
	char nodeName[256];
	
	dynamic_reconfigure::Server<thermalvis::extractorConfig> server;
	dynamic_reconfigure::Server<thermalvis::extractorConfig>::CallbackType f;
	
	float** ApplyBilateralFilter(float** data, const int width, const int height, float sigmaP, float sigmaR);
	
	FILE *dataFile, *timeFile, *depthtimeFile, *depthInternaltimeFile, *imgtimeFile, *imgInternaltimeFile, *thermaltimeFile, *thermalInternaltimeFile, *thermalDuplicatesFile;
	
	int count;
	int filenum;
	double startTime;
	int skipFrames;
	int maxFrames;

	float d_fx;
	float d_fy;
	float d_cx;
	float d_cy;
	
  char *outputFilename;
  char *outputFilenameThermal;
  char *outputdepth, *outputcloud ;
  
  stringstream imgPath;
    stringstream timePath;
    stringstream imgtimePath, imgInternaltimePath, depthtimePath, depthInternaltimePath, thermaltimePath, thermalInternaltimePath, thermalDuplicatesPath;
    
    // Image topics to load
  std::vector<std::string> topics;
  
  
  
  int count_rgb; 
  int count_depth;
  int count_rgbinfo; 
  int count_depthinfo;
  int count_thermal;
  int count_thermalinfo;
  
  sensor_msgs::Image::ConstPtr rgbImg;
  sensor_msgs::CameraInfo::ConstPtr rgbInfo;
  sensor_msgs::Image::ConstPtr depthImg;
  sensor_msgs::CameraInfo::ConstPtr depthInfo;
  sensor_msgs::Image::ConstPtr thermalImg;
  sensor_msgs::CameraInfo::ConstPtr thermalInfo;
  
	
public:

	rosbag::View *view;

	extractorNode(ros::NodeHandle& nh, extractorData startupData);
	~extractorNode();
	
	void makeDirectories();
	
	bool isStillListening();
	
	void serverCallback(thermalvis::extractorConfig &config, uint32_t level);
	
	
	
	void extractBagFile();
	void cameraCallback(const sensor_msgs::Image::ConstPtr& rgbImgMsg,
					  const sensor_msgs::Image::ConstPtr& depthImgMsg,
					  const sensor_msgs::Image::ConstPtr& thermalImgMsg,
					  const sensor_msgs::CameraInfo::ConstPtr& rgbInfoMsg,
					  const sensor_msgs::CameraInfo::ConstPtr& depthInfoMsg,
					  const sensor_msgs::CameraInfo::ConstPtr& thermalInfoMsg
					  );
};

typedef message_filters::sync_policies::ApproximateTime< sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::CameraInfo, sensor_msgs::CameraInfo > SyncPolicy;

/**
 * Inherits from message_filters::SimpleFilter<M>
 * to use protected signalMessage function 
 */
template <class M>
class BagSubscriber : public message_filters::SimpleFilter<M>
{
public:
  void newMessage(const boost::shared_ptr<M const> &msg)
  {
    signalMessage(msg);
  }
};

#endif
